#include "kalman_filter.h"

// 初始化参数
KalmanFilter::KalmanFilter(int state_dim, int meas_dim) : state_dim(state_dim), meas_dim(meas_dim)
{
    x.setZero(state_dim);
    P.setIdentity(state_dim, state_dim);
    Q.setIdentity(state_dim, state_dim);
    R.setIdentity(meas_dim, meas_dim);
    F.setIdentity(meas_dim, state_dim);
    H.setZero(meas_dim, state_dim);
    B.setZero(state_dim, state_dim);
}

KalmanFilter::~KalmanFilter()
{
}

void KalmanFilter::init(const Eigen::VectorXd &x0, const Eigen::MatrixXd &P0,
                        const Eigen::MatrixXd &Q_in, const Eigen::MatrixXd &R_in,
                        const Eigen::MatrixXd &F_in, const Eigen::MatrixXd &H_in)
{
    x = x0;
    P = P0;
    Q = Q_in;
    R = R_in;
    F = F_in;
    H = H_in;
}

void KalmanFilter::predict(const Eigen::VectorXd &u)
{
    // 状态预测
    x = F * x + B * u;

    // 协方差
    P = F * P * F.transpose() + Q;
}

void KalmanFilter::update(const Eigen::VectorXd &z)
{
    Eigen::VectorXd y = z - H * x;

    Eigen::MatrixXd S = H * P * H.transpose() + R;
    Eigen::MatrixXd K = P * H.transpose() * S.inverse();
    x = x + K * y;
    P = (Eigen::MatrixXd::Identity(state_dim, state_dim) - K * H) * P;
}

Eigen::VectorXd KalmanFilter::getState() const
{
    return x;
}
